#include "Motor.h"

/******************备注******************
    Motor1_PIN    ->    GPIO_Pin_6
    Motor2_PIN    ->    GPIO_Pin_7
    Motor3_PIN    ->    GPIO_Pin_0
    Motor4_PIN    ->    GPIO_Pin_1
    PA6  -> TIM3_CH1
    PA7  -> TIM3_CH2
    PB0  -> TIM3_CH3
    PB1  -> TIM3_CH4
******************************************/
void Motor_Init(void)
{
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOA, ENABLE);
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);

    TIM_OCInitTypeDef TIM_OCInitStructure;
    GPIO_InitTypeDef GPIO_InitStructure;
    TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Pin = Motor1_PIN | Motor2_PIN;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOA, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
    GPIO_InitStructure.GPIO_Pin = Motor3_PIN | Motor4_PIN;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);

    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_PWM1;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    TIM_OCInitStructure.TIM_Pulse = 0;
    TIM_OC1Init(TIM3, &TIM_OCInitStructure);
    TIM_OC2Init(TIM3, &TIM_OCInitStructure);
    TIM_OC3Init(TIM3, &TIM_OCInitStructure);
    TIM_OC4Init(TIM3, &TIM_OCInitStructure);
    TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_OC2PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_OC3PreloadConfig(TIM3, TIM_OCPreload_Enable);
    TIM_OC4PreloadConfig(TIM3, TIM_OCPreload_Enable);

    TIM_TimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    TIM_TimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInitStructure.TIM_Period = 1000 - 1;        //ARR
    TIM_TimeBaseInitStructure.TIM_Prescaler = 100 - 1;      //PSC
    TIM_TimeBaseInit(TIM3, &TIM_TimeBaseInitStructure);
    TIM_Cmd(TIM3, ENABLE);
}

/**
 * @brief PWM设置 & 限幅
 * 
 * @param M1_PWM 右上角电机
 * @param M2_PWM 左上角电机
 * @param M3_PWM 
 * @param M4_PWM 
 */
void Motor_PWM(int16_t M1_PWM, int16_t M2_PWM, int16_t M3_PWM, int16_t M4_PWM)
{
    if (M1_PWM >= Motor_PWM_MAX) M1_PWM = Motor_PWM_MAX;
    if (M2_PWM >= Motor_PWM_MAX) M2_PWM = Motor_PWM_MAX;
    if (M3_PWM >= Motor_PWM_MAX) M3_PWM = Motor_PWM_MAX;
    if (M4_PWM >= Motor_PWM_MAX) M4_PWM = Motor_PWM_MAX;
    if (M2_PWM <= Motor_PWM_MIN) M2_PWM = Motor_PWM_MIN;
    if (M2_PWM <= Motor_PWM_MIN) M2_PWM = Motor_PWM_MIN;
    if (M2_PWM <= Motor_PWM_MIN) M2_PWM = Motor_PWM_MIN;
    if (M2_PWM <= Motor_PWM_MIN) M2_PWM = Motor_PWM_MIN;

	TIM3->CCR1 = M1_PWM;
	TIM3->CCR2 = M2_PWM;
	TIM3->CCR3 = M3_PWM;
	TIM3->CCR4 = M4_PWM;
}
